/*
 * @file	sensor_support.c
 * @autohr  Queclink beck.wan
 * @date	2019年4月13日
 * @brief
 *
 * Coypright (C) 2017 Queclink Co., Ltd. All Rights Reserved
 *
 * @attention
 *
 * @usage
 *
 * @record
 *
 */
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <sensor/sensor_wrapper.h>
#include <sensor/sensor_support.h>
#include <public/errno.h>
#include "sensor_log.h"
#include <sensor/icm42670/sensor_icm42670.h>

#include "bsp/inc/bsp_gsensor.h"



struct sensor_ops snsradp = {0};    /* sensor spi&i2c接口适配 */

snsr_support_t qlsdk_sensor_support = {
    .chip_valid = false,
    .gyro_use   = false,
    .range_check = false,
    .reset_tick = 0,
    .self_check = 0,
    .accel_range = 2,
    .gyro_range = 250,
    .cur_odr    = 100,
    .new_odr    = 100,
    .zero_offset = {
        .x  = 0.0,
        .y  = 0.0,
        .z  = 0.0
    },
    .lwp_check_time = 1,        /* 自检间隔，默认1min */
};

const sensor_function_t *hal_snsr_func = NULL;



/* sensor SPI读接口 */
int sensor_spi_read_reg(unsigned int addr, unsigned char addr_len, unsigned char *data, unsigned int len)
{
    int ret = 0, i;
    unsigned char val;
    
    snsradp.spi.select();

    for(i = 0; i < addr_len; i++) {
        val = (addr >> (addr_len - 1 - i) * 8) & 0xFF;
        ret += snsradp.spi.write(&val, 1);
    }

    if(len > 0)
        ret += snsradp.spi.read(data, len);
    
    snsradp.spi.deselect();

    
    return ret;
}

/* sensor SPI写接口 */
int sensor_spi_write_reg(unsigned int addr, unsigned char addr_len, unsigned char *data, unsigned int len)
{
    int ret = 0, i;
    unsigned char val;

    snsradp.spi.select();

    for(i = 0; i < addr_len; i++) {
        val = (addr >> (addr_len - 1 - i) * 8) & 0xFF;
        ret += snsradp.spi.write(&val, 1);
    }

    if(len > 0)
        ret += snsradp.spi.write(data, len);
    snsradp.spi.deselect();

    
    return ret;
}



/* 存储ACC量程配置 */
void sensor_accel_range_set(unsigned char range)
{
    if ((2 != range) && (4 != range) && (8 != range) && (16 != range))
        range = 2;

    qlsdk_sensor_support.accel_range = range;
}

int sensor_lwp_check_time_set(int time)
{
    if (time < 0)
        return -EINVAL;
    qlsdk_sensor_support.lwp_check_time = time;
    return 0;
}

void sensor_gyro_range_set(unsigned short range)
{
    if(125 != range && 250 != range && 500 != range && 1000 != range && 2000 != range)
        range = 250;

    qlsdk_sensor_support.gyro_range = range;
}

int sensor_accel_odr_set(unsigned short odr)
{
    if(100 != odr && 200 != odr && 400 != odr)
        return -EINVAL;

    qlsdk_sensor_support.new_odr = odr;
    return 0;
}

/* 获取1G表示的量程值,供senosr中间层使用 */
int sensor_accel_1g_range_count(void)
{
    if(NULL == hal_snsr_func)
        return -EINVAL;

    return hal_snsr_func->max_accel_value() / qlsdk_sensor_support.accel_range;
}

/* 获取1°/s表示的量程值 */
int sensor_gyro_1dge_per_s_range_count(void)
{
    unsigned int max_value = 0;
    if(NULL == hal_snsr_func)
        return -EINVAL;

    if (NULL != hal_snsr_func->max_gyro_value)
        max_value = hal_snsr_func->max_gyro_value();
    else
        max_value = hal_snsr_func->max_accel_value();

    return max_value / qlsdk_sensor_support.gyro_range;
}

int sensor_zero_offset_set(float x, float y, float z)
{
    qlsdk_sensor_support.zero_offset.x = x;
    qlsdk_sensor_support.zero_offset.y = y;
    qlsdk_sensor_support.zero_offset.z = z;
    return 0;
}


extern const sensor_function_t qlsdk_sensor_function_table[];



void snsr_adapter_init(void)
{
    struct sensor_ops *adp = &snsradp;

    adp->spi.select  = gsensor_spi_select; 
    adp->spi.deselect  = gsensor_spi_deselect; 
    adp->spi.write   = gsensor_spi_write; 
    adp->spi.read    = gsensor_spi_read; 
    adp->int1_enable  = gsensor_int1_enable; 
    adp->int1_disable = gsensor_int1_disable; 
    adp->extra_sleep_en = gsensor_int1_extra_sleep_en; 
    adp->is_spi = 1; 

}

int sensor_initial(void)
{
    memset(&snsradp, 0, sizeof(struct sensor_ops));

    //bsp in
    snsr_adapter_init();

    snsradp.spi.deselect();

    //app interface out
    hal_snsr_func = qlsdk_sensor_function_table;
    
    return 0;
}


const sensor_function_t qlsdk_sensor_function_table[] = 
{
    {
        .initialize         = icm42670_sensor_initialize,
        .lowpower           = icm42670_sensor_lowpower_initialize,
        .chip_id            = icm42670_sensor_chip_id_get,
        .temperature        = icm42670_sensor_temperature_get,
        .accel_frame_get    = icm42670_sensor_accel_frame_get,
        .gyro_frame_get     = icm42670_sensor_gyro_frame_get,
        .max_accel_value    = icm42670_sensor_max_accel_value,
        .odr_get            = NULL,
        .max_gyro_value     = NULL,
    },

    NULL,
};





/**
 * @brief 初始化
 */
void sensor_initialize(void) 
{
    if (hal_snsr_func) {
        hal_snsr_func->initialize();
    }
}

/**
 * @brief 低功耗初始化
 */
void sensor_lowpower_initialize(void) 
{
    if (hal_snsr_func) {
        hal_snsr_func->lowpower();
    }
}

